#region Using Statements
using System;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using System.Collections.Generic;
using SkinnedModel;

#endregion

namespace SkinnedModel
{
    

    //The idea is that this class houses the iksolver and the animation player
    //eventually it'll be a controller to blend between them as needed
    public class IKAnimObject
    {

        protected Matrix worldMatrix;
        public Model currentModel;
        protected AnimationPlayer animationPlayer;
        protected IKSolver ikSolver;
        protected AnimationClip[] clip = null;
        protected Vector3 trajectory;
        protected Vector3 velocity;
        
        protected string testEffector;

        protected float[] jointBlends;

        public Dictionary<string, EffectorMotion> effectorMotions = new Dictionary<string, EffectorMotion>();

        public IKAnimObject()
        {
            worldMatrix = Matrix.Identity;
        }

        public void InitAnimation(Model cm, ref string[] tracks)
        {
            currentModel = cm;
            SkinningData skinningData = currentModel.Tag as SkinningData;

            if (skinningData == null)
                throw new InvalidOperationException
                    ("This model does not contain a SkinningData tag.");


            // Create an animation player, and start decoding an animation clip.
            animationPlayer = new AnimationPlayer(skinningData);
            ikSolver = new IKSolver(skinningData);
            

            int nTakes;
            nTakes = tracks.Length;
            clip = new AnimationClip[nTakes];
            for (int i = 0; i < tracks.Length; i++)
            {
                clip[i] = skinningData.AnimationClips[tracks[i]];
            }
            animationPlayer.StartClip(clip[0]);
            ikSolver.SetPoseFromFirstKey(clip[0]);

            jointBlends = new float[skinningData.BoneNames.Count];
            clearJointBlends();
        }

        
        //clear out
        //TODO: better way than this
        //Anim = 0, ik = 1
        protected void clearJointBlends()
        {
            for (int i = 0; i < jointBlends.Length; i++)
            {
                jointBlends[i] = 0.0f;
            }
        }

        protected void addIKControl()
        {

        }

        protected void UpdateAnim(TimeSpan elapsedTime)
        {
            //there may be a initial loop 
            //between initalize and loadcontent
            if (ikSolver == null)
                return;

            long ticks = (long)(((float)elapsedTime.Ticks));
            elapsedTime = new TimeSpan(ticks);

            //first step is to update the anim player
            animationPlayer.Update(elapsedTime, true, worldMatrix);

            //second step is to get anim joints
            Matrix[] animMatrices = animationPlayer.GetBoneTransforms();

            //TODO: get this set up for blending lower/upper motion
            ikSolver.UpdateMatrices(ref animMatrices, ref jointBlends);
            ikSolver.Update(elapsedTime, ref worldMatrix);

            //Update Root Position
            UpdatePosition(elapsedTime);
        }

        /// <summary>
        ///Updates root position        
        /// </summary>
        protected void UpdatePosition(TimeSpan elapsedTime)
        {

            if (trajectory.Length() > 0)
            {
                velocity.Z = -10.0f;
                Vector3 trans = velocity;
                trans *= (float)elapsedTime.TotalSeconds;
                this.worldMatrix.Translation += trans;
                trajectory -= trans;
                if (trajectory.Z > 0)
                    trajectory.Z = 0;
            }
            else
            {
                velocity = Vector3.Zero;
            }
        }



        public Matrix GetMatrix()
        {
            return worldMatrix;
        }

        //TEMP HACK
        //TODO: remove this
        public void DoSolve()
        {
            //TEMP HACK
            TimeSpan t = new TimeSpan();
            ikSolver.Update(t, ref worldMatrix);
        }


        public ModelMeshCollection GetMeshes(){
            return currentModel.Meshes;
        }

        //Here's our blend
        //IK trumps Anim
        public Matrix[] GetSkinTransorms()
        {
            
            Matrix[] ikMatrices = ikSolver.GetSkinTransforms();            
            Matrix[] animMatrices = animationPlayer.GetSkinTransforms();
            

            for(int i = 0; i < jointBlends.Length; i++){
                if(jointBlends[i] == 1.0)
                    animMatrices[i] = ikMatrices[i];
            }
              
            return animMatrices;

        }

        public void SetMatrix(Matrix m)
        {
            worldMatrix = m;
        }

        public void StartClip()
        {
            //ikSolver.SetPoseFromFirstKey(clip[0]);
            animationPlayer.StartClip(clip[0]);
        }

        public void StartClip(int idx)
        {
            if (idx == -1)
            {
                animationPlayer.StopClip();
            }
            else
            {
                animationPlayer.StartClip(clip[idx]);
                //ikSolver.SetPoseFromFirstKey(clip[idx]);
            }
        }

        public AnimationClip GetClip(int idx)
        {
            return clip[idx];
        }

        /*public void SetModel(Model model)
        {
            currentModel = model;
        }*/

        /// <summary>
        /// Updates Effector Motions
        /// Removes Completed Motions from Queue
        /// </summary>
        /// <param name="elapsedTime"></param>
        protected void UpdateEffectorMotions(TimeSpan elapsedTime)
        {

            List<string> toRemove = new List<string>();
            foreach (string s in this.effectorMotions.Keys)
            {
                EffectorMotion e = this.effectorMotions[s];

                //e.UpdatePosition(elapsedTime.Seconds);
                
                e.ExtractIKGoal(ref ikSolver, elapsedTime);

                /*
                    ikSolver.AddOneJointAimGoal(s, target);                    
                }
                else
                {
                    ikSolver.AddReachGoal(s, e.getRoot(), target);
                }
                 * */

                if (e.IsFinished())
                {
                    toRemove.Add(s);
                }
            }

            foreach (string s in toRemove)
            {
                effectorMotions.Remove(s);
            }

        }

    }


}